diff --git a/cpu/mpc8xx/interrupts.c b/cpu/mpc8xx/interrupts.c
index 57ff4dd..8664826 100644
--- a/cpu/mpc8xx/interrupts.c
+++ b/cpu/mpc8xx/interrupts.c
@@ -28,99 +28,104 @@
 #include <asm/processor.h>
 #include <commproc.h>
 
-/****************************************************************************/
+/************************************************************************/
 
-unsigned decrementer_count;		/* count value for 1e6/HZ microseconds */
+unsigned decrementer_count;	/* count value for 1e6/HZ microseconds	*/
 
-/****************************************************************************/
+/************************************************************************/
 
 /*
  * CPM interrupt vector functions.
  */
-struct	cpm_action {
-	 interrupt_handler_t *handler;
-	 void *arg;
+struct interrupt_action {
+	interrupt_handler_t *handler;
+	void *arg;
 };
 
-static struct cpm_action cpm_vecs[CPMVEC_NR];
+static struct interrupt_action cpm_vecs[CPMVEC_NR];
+static struct interrupt_action irq_vecs[NR_IRQS];
 
 static void cpm_interrupt_init (void);
-static void cpm_interrupt(int irq, struct pt_regs * regs);
+static void cpm_interrupt (void *regs);
 
-/****************************************************************************/
+/************************************************************************/
 
-static __inline__ unsigned long get_msr(void)
+static __inline__ unsigned long get_msr (void)
 {
-    unsigned long msr;
+	unsigned long msr;
 
-    asm volatile("mfmsr %0" : "=r" (msr) :);
-    return msr;
+	asm volatile ("mfmsr %0":"=r" (msr):);
+
+	return msr;
 }
 
-static __inline__ void set_msr(unsigned long msr)
+static __inline__ void set_msr (unsigned long msr)
 {
-    asm volatile("mtmsr %0" : : "r" (msr));
+	asm volatile ("mtmsr %0"::"r" (msr));
 }
 
-static __inline__ unsigned long get_dec(void)
+static __inline__ unsigned long get_dec (void)
 {
-    unsigned long val;
+	unsigned long val;
 
-    asm volatile("mfdec %0" : "=r" (val) :);
-    return val;
+	asm volatile ("mfdec %0":"=r" (val):);
+
+	return val;
 }
 
 
-static __inline__ void set_dec(unsigned long val)
+static __inline__ void set_dec (unsigned long val)
 {
-    asm volatile("mtdec %0" : : "r" (val));
+	asm volatile ("mtdec %0"::"r" (val));
 }
 
 
 void enable_interrupts (void)
 {
-	set_msr (get_msr() | MSR_EE);
+	set_msr (get_msr () | MSR_EE);
 }
 
 /* returns flag if MSR_EE was set before */
 int disable_interrupts (void)
 {
-	ulong msr = get_msr();
+	ulong msr = get_msr ();
+
 	set_msr (msr & ~MSR_EE);
 	return ((msr & MSR_EE) != 0);
 }
 
-/****************************************************************************/
+/************************************************************************/
 
-int interrupt_init(void)
+int interrupt_init (void)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
 
-	decrementer_count = get_tbclk() / CFG_HZ;
+	decrementer_count = get_tbclk () / CFG_HZ;
 
-	cpm_interrupt_init();
+	/* disable all interrupts */
+	immr->im_siu_conf.sc_simask = 0;
 
-	/* disable all interrupts except for the CPM interrupt */
-	immr->im_siu_conf.sc_simask = 1 << (31-CPM_INTERRUPT);
+	/* Configure CPM interrupts */
+	cpm_interrupt_init ();
 
 	set_dec (decrementer_count);
 
-	set_msr (get_msr() | MSR_EE);
+	set_msr (get_msr () | MSR_EE);
 
 	return (0);
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 /*
  * Handle external interrupts
  */
-void external_interrupt(struct pt_regs *regs)
+void external_interrupt (struct pt_regs *regs)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
-	int	irq;
-	ulong	simask, newmask;
-	ulong	vec, v_bit;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+	int irq;
+	ulong simask, newmask;
+	ulong vec, v_bit;
 
 	/*
 	 * read the SIVEC register and shift the bits down
@@ -137,14 +142,15 @@
 	newmask = simask & (~(0xFFFF0000 >> irq));
 	immr->im_siu_conf.sc_simask = newmask;
 
-	if (!(irq & 0x1)) {			/* External Interrupt ?		*/
+	if (!(irq & 0x1)) {		/* External Interrupt ?     */
 		ulong siel;
+
 		/*
 		 * Read Interrupt Edge/Level Register
 		 */
 		siel = immr->im_siu_conf.sc_siel;
 
-		if (siel & v_bit) {		/* edge triggered interrupt ?	*/
+		if (siel & v_bit) {	/* edge triggered interrupt ?   */
 			/*
 			 * Rewrite SIPEND Register to clear interrupt
 			 */
@@ -152,34 +158,29 @@
 		}
 	}
 
-	switch (irq) {
-	case CPM_INTERRUPT:
-		cpm_interrupt (irq, regs);
-		break;
-	default:
+	if (irq_vecs[irq].handler != NULL) {
+		irq_vecs[irq].handler (irq_vecs[irq].arg);
+	} else {
 		printf ("\nBogus External Interrupt IRQ %d Vector %ld\n",
-			irq, vec);
+				irq, vec);
 		/* turn off the bogus interrupt to avoid it from now */
 		simask &= ~v_bit;
-		break;
 	}
-
 	/*
 	 * Re-Enable old Interrupt Mask
 	 */
 	immr->im_siu_conf.sc_simask = simask;
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 /*
  * CPM interrupt handler
  */
-static void
-cpm_interrupt(int irq, struct pt_regs * regs)
+static void cpm_interrupt (void *regs)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
-	uint	vec;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+	uint vec;
 
 	/*
 	 * Get the vector by setting the ACK bit
@@ -190,13 +191,14 @@
 	vec >>= 11;
 
 	if (cpm_vecs[vec].handler != NULL) {
-		(*cpm_vecs[vec].handler)(cpm_vecs[vec].arg);
+		(*cpm_vecs[vec].handler) (cpm_vecs[vec].arg);
 	} else {
 		immr->im_cpic.cpic_cimr &= ~(1 << vec);
 		printf ("Masking bogus CPM interrupt vector 0x%x\n", vec);
 	}
 	/*
-	 * After servicing the interrupt, we have to remove the status indicator.
+	 * After servicing the interrupt,
+	 * we have to remove the status indicator.
 	 */
 	immr->im_cpic.cpic_cisr |= (1 << vec);
 }
@@ -207,75 +209,110 @@
  * to do is ACK it and return. This is a no-op function so we don't
  * need any special tests in the interrupt handler.
  */
-static void
-cpm_error_interrupt (void *dummy)
+static void cpm_error_interrupt (void *dummy)
 {
 }
 
-/****************************************************************************/
-
+/************************************************************************/
 /*
- * Install and free a CPM interrupt handler.
+ * Install and free an interrupt handler
  */
-
-void
-irq_install_handler(int vec, interrupt_handler_t *handler, void *arg)
+void irq_install_handler (int vec, interrupt_handler_t * handler,
+						  void *arg)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
 
-	if (cpm_vecs[vec].handler != NULL) {
-		printf ("CPM interrupt 0x%x replacing 0x%x\n",
-			(uint)handler, (uint)cpm_vecs[vec].handler);
+	if ((vec & CPMVEC_OFFSET) != 0) {
+		/* CPM interrupt */
+		vec &= 0xffff;
+		if (cpm_vecs[vec].handler != NULL) {
+			printf ("CPM interrupt 0x%x replacing 0x%x\n",
+				(uint) handler,
+				(uint) cpm_vecs[vec].handler);
+		}
+		cpm_vecs[vec].handler = handler;
+		cpm_vecs[vec].arg = arg;
+		immr->im_cpic.cpic_cimr |= (1 << vec);
+#if 0
+		printf ("Install CPM interrupt for vector %d ==> %p\n",
+			vec, handler);
+#endif
+	} else {
+		/* SIU interrupt */
+		if (irq_vecs[vec].handler != NULL) {
+			printf ("SIU interrupt %d 0x%x replacing 0x%x\n",
+				vec,
+				(uint) handler,
+				(uint) cpm_vecs[vec].handler);
+		}
+		irq_vecs[vec].handler = handler;
+		irq_vecs[vec].arg = arg;
+		immr->im_siu_conf.sc_simask |= 1 << (31 - vec);
+#if 0
+		printf ("Install SIU interrupt for vector %d ==> %p\n",
+			vec, handler);
+#endif
 	}
-	cpm_vecs[vec].handler = handler;
-	cpm_vecs[vec].arg     = arg;
-	immr->im_cpic.cpic_cimr |= (1 << vec);
-#if 0
-	printf ("Install CPM interrupt for vector %d ==> %p\n", vec, handler);
-#endif
 }
 
-void
-irq_free_handler(int vec)
+void irq_free_handler (int vec)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+
+	if ((vec & CPMVEC_OFFSET) != 0) {
+		/* CPM interrupt */
+		vec &= 0xffff;
 #if 0
-	printf ("Free CPM interrupt for vector %d ==> %p\n",
-		vec, cpm_vecs[vec].handler);
+		printf ("Free CPM interrupt for vector %d ==> %p\n",
+			vec, cpm_vecs[vec].handler);
 #endif
-	immr->im_cpic.cpic_cimr &= ~(1 << vec);
-	cpm_vecs[vec].handler = NULL;
-	cpm_vecs[vec].arg     = NULL;
+		immr->im_cpic.cpic_cimr &= ~(1 << vec);
+		cpm_vecs[vec].handler = NULL;
+		cpm_vecs[vec].arg = NULL;
+	} else {
+		/* SIU interrupt */
+#if 0
+		printf ("Free CPM interrupt for vector %d ==> %p\n",
+			vec, cpm_vecs[vec].handler);
+#endif
+		immr->im_siu_conf.sc_simask &= ~(1 << (31 - vec));
+		irq_vecs[vec].handler = NULL;
+		irq_vecs[vec].arg = NULL;
+	}
 }
 
-/****************************************************************************/
+/************************************************************************/
 
-static void
-cpm_interrupt_init (void)
+static void cpm_interrupt_init (void)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
 
 	/*
 	 * Initialize the CPM interrupt controller.
 	 */
 
 	immr->im_cpic.cpic_cicr =
-		( CICR_SCD_SCC4 |
-		  CICR_SCC_SCC3 |
-		  CICR_SCB_SCC2 |
-		  CICR_SCA_SCC1 ) | ((CPM_INTERRUPT/2) << 13) | CICR_HP_MASK;
+		(CICR_SCD_SCC4 |
+		 CICR_SCC_SCC3 |
+		 CICR_SCB_SCC2 |
+		 CICR_SCA_SCC1) | ((CPM_INTERRUPT / 2) << 13) | CICR_HP_MASK;
 
 	immr->im_cpic.cpic_cimr = 0;
 
 	/*
 	 * Install the error handler.
 	 */
-	irq_install_handler(CPMVEC_ERROR, cpm_error_interrupt, NULL);
+	irq_install_handler (CPMVEC_ERROR, cpm_error_interrupt, NULL);
 
 	immr->im_cpic.cpic_cicr |= CICR_IEN;
+
+	/*
+	 * Install the cpm interrupt handler
+	 */
+	irq_install_handler (CPM_INTERRUPT, cpm_interrupt, NULL);
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 volatile ulong timestamp = 0;
 
@@ -284,18 +321,19 @@
  * with interrupts disabled.
  * Trivial implementation - no need to be really accurate.
  */
-void timer_interrupt(struct pt_regs *regs)
+void timer_interrupt (struct pt_regs *regs)
 {
-	volatile immap_t *immr = (immap_t *)CFG_IMMR;
+	volatile immap_t *immr = (immap_t *) CFG_IMMR;
+
 #ifdef CONFIG_STATUS_LED
-    	extern void status_led_tick (ulong);
+	extern void status_led_tick (ulong);
 #endif
 #if 0
 	printf ("*** Timer Interrupt *** ");
 #endif
 	/* Reset Timer Expired and Timers Interrupt Status */
 	immr->im_clkrstk.cark_plprcrk = KAPWR_KEY;
-	__asm__("nop");
+	__asm__ ("nop");
 	immr->im_clkrst.car_plprcr |= PLPRCR_TEXPS | PLPRCR_TMIST;
 	/* Restore Decrementer Count */
 	set_dec (decrementer_count);
@@ -304,11 +342,10 @@
 
 #ifdef CONFIG_STATUS_LED
 	status_led_tick (timestamp);
-#endif	/* CONFIG_STATUS_LED */
+#endif /* CONFIG_STATUS_LED */
 
 #if defined(CONFIG_WATCHDOG) || defined(CFG_CMA_LCD_HEARTBEAT)
 
-
 	/*
 	 * The shortest watchdog period of all boards (except LWMON)
 	 * is approx. 1 sec, thus re-trigger watchdog at least
@@ -321,20 +358,20 @@
 #endif
 
 #if defined(CFG_CMA_LCD_HEARTBEAT)
-		extern void lcd_heartbeat(void);
-		lcd_heartbeat();
+		extern void lcd_heartbeat (void);
+
+		lcd_heartbeat ();
 #endif /* CFG_CMA_LCD_HEARTBEAT */
 
 #if defined(CONFIG_WATCHDOG)
-		reset_8xx_watchdog(immr);
+		reset_8xx_watchdog (immr);
 #endif /* CONFIG_WATCHDOG */
 
 	}
-
 #endif /* CONFIG_WATCHDOG || CFG_CMA_LCD_HEARTBEAT */
 }
 
-/****************************************************************************/
+/************************************************************************/
 
 void reset_timer (void)
 {
@@ -351,4 +388,4 @@
 	timestamp = t;
 }
 
-/****************************************************************************/
+/************************************************************************/
diff --git a/cpu/mpc8xx/start.S b/cpu/mpc8xx/start.S
index a430061..a1b7eff 100644
--- a/cpu/mpc8xx/start.S
+++ b/cpu/mpc8xx/start.S
@@ -285,7 +285,14 @@
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r12,0xd00-4*3		/* save LR & SRRx */
+	li	r20,0xd00-4		/* Get stack pointer */
+	lwz	r12,0(r20)
+	subi	r12,r12,12		/* Adjust stack pointer */
+	li	r0,0xc00+_end_back-SystemCall
+	cmplw	0, r0, r12		/* Check stack overflow */
+	bgt	1f
+	stw	r12,0(r20)
+
 	mflr	r0
 	stw	r0,0(r12)
 	mfspr	r0,SRR0
@@ -310,7 +317,9 @@
 	mtmsr	r11
 	SYNC
 
-	li	r12,0xd00-4*3		/* restore regs */
+	li	r12,0xd00-4		/* restore regs */
+	lwz	r12,0(r12)
+
 	lwz	r11,0(r12)
 	mtlr	r11
 	lwz	r11,4(r12)
@@ -318,8 +327,13 @@
 	lwz	r11,8(r12)
 	mtspr	SRR1,r11
 
+	addi	r12,r12,12		/* Adjust stack pointer */
+	li	r20,0xd00-4
+	stw	r12,0(r20)
+
 	SYNC
 	rfi
+_end_back:
 
 	STD_EXCEPTION(0xd00, SingleStep, UnknownException)